/*
                     ---   PROPRIETARY NOTICE   ---


     The information contained herein is proprietary to Smiths Aerospace
     and shall not be reproduced or disclosed in whole or in part or used
     for any design or manufacture except when such user possesses direct
     written authorization from Smiths Aerospace.


     Software Common Operating Environment (SCOE)
     Copyright 2004-2006, Smiths Aerospace.

  ------------------------------------------------------------------------------
  %PCMS_HEADER_SUBSTITUTION_START%
   MODULE : %PM%

   HISTORY:
    %PL%

  %PCMS_HEADER_SUBSTITUTION_END%
  ------------------------------------------------------------------------------
*/

/* Significant Changes for SL2:
-> Data structures are now based on up-to-date versions of D6-16Z002-1 and the RDC IO Catalog
    - All messages now conform to current version of the spec
    - Switch status is based on RDC template 13023
    - Motor command & status are based on RDC template 11022 
-> Moved structures, types, constants, macros, and prototypes specific to the motor controller to "MotorController.h"
-> Moved RDC and D6-16Z002-1 structures, types, constants, and macros into their own header files
-> Message sizees were updated to match the new RDC message structures
-> "toggleMotor" replaced with "commandMotor"
    - "commandMotor" doesn't need to check the motor's status, eliminating an OS service call
-> checkCode macro was replaced with the checkCode function, to improve cacheability
*/

/* includes */

#include <arpa/inet.h>

#include "apex/apexLib.h"

#include "stdio.h"
#include "string.h"
#include "math.h"
#include "stdlib.h"
#include "scoeAmioEnable.h"
#include "MotorController.h"

/***************** 
Model header file 
*/
#include "AswTest.h"                   
#include <unistd.h>

extern void parse_can_message1(uint8_t* data1, uint32_t length, B_AswTest_T* message);
extern void parse_can_message2(uint8_t* data1, uint32_t length, B_AswTest_T* message);

typedef struct {
  unsigned int id;
  unsigned char data[8];
  unsigned int len;
} CAN_Packet;




#define LC_DEBUG_MODE 

/* Port IDs */
/* "LOCAL" is defined as "static" by the system headers (these variables 
 *  exist only within the scope of this file) 
 */
LOCAL QUEUING_PORT_ID_TYPE qMotorCommand_Out_ID;
LOCAL SAMPLING_PORT_ID_TYPE sMotorStatus_In_ID, sSwitchStatus_In_ID;

/* codeToStr: Returns error strings for A653 error codes */
const char * codeToStr ( const RETURN_CODE_TYPE retCode )
{
    const char * returnValue = NULL;
    static const char * const returnTable[] =
    {
        "NO_ERROR",
        "NO_ACTION",
        "NOT_AVAILABLE",
        "INVALID_PARAM",
        "INVALID_CONFIG",
        "INVALID_MODE",
        "TIMED_OUT"
    };
    static const char * const unknownError = "Unknown Error";
    
    /* RETURN_CODE_TYPE has range 0-6, so "Unknown Error" should
     * =never= occur
     */
    if( (retCode >= 0) && (retCode <= LC_MAX_A653_ERR_VAL) )
    {
        returnValue = returnTable[retCode];
    }
    else
    {
        returnValue = unknownError;
    }

    return returnValue;
}

/* checkCode: Checks return codes for ARINC functions, prints an error for
 *            values other than "NO_ERROR" */
void checkCode(const char * const msg, const RETURN_CODE_TYPE code)
{
    if (code != NO_ERROR)
    {
        printf ("%s error : %s\n", msg, codeToStr(code));
    }
}

/****************************************************************\
Begin
\****************************************************************/
/* Example use case for call to exported function: Input_10ms */
extern void sample_usage_Input_10ms(void);
void sample_usage_Input_10ms(void)
{
  /* Set task inputs here */

  /* Call to exported function */
  Input_10ms();

  /* Read function outputs here */
}

/* Example use case for call to exported function: Strgy_10ms */
extern void sample_usage_Strgy_10ms(void);
void sample_usage_Strgy_10ms(void)
{
  /* Set task inputs here */

  /* Call to exported function */
  Strgy_10ms();

  /* Read function outputs here */
}

/* Example use case for call to exported function: Strgy_20ms */
extern void sample_usage_Strgy_20ms(void);
void sample_usage_Strgy_20ms(void)
{
  /* Set task inputs here */

  /* Call to exported function */
  Strgy_20ms();

  /* Read function outputs here */
}

/* Example use case for call to exported function: Output_10ms */
extern void sample_usage_Output_10ms(void);
void sample_usage_Output_10ms(void)
{
  /* Set task inputs here */

  /* Call to exported function */
  Output_10ms();

  /* Read function outputs here */
}

//lihy
void SetBitOfU8Value(uint8_T * pbyData, uint8_T n, uint8_T byOffset, uint8_T byValue)
{
    uint8_T tmp = *pbyData;
    uint8_T mask;

    mask = (((uint8_T)1u << n) - 1) << byOffset;
    tmp &= ~mask;
    tmp |= (byValue << byOffset) & mask;
    *pbyData |= tmp;
}

#define GetCanSigMsg(littleEndian, isSigned, ValueType) ((((littleEndian)&0xFFUL) << 16) | \
                                                            (((isSigned)&0xFFUL) << 8) | \
                                                            ((ValueType)&0xFFUL))

void CanPack(real32_T value, uint8_T *data, uint8_T startBit, uint8_T BitLength,
                    uint32_T ValueMsg, real32_T factor, real32_T offset)
{
    real32_T tmp;
    int32_T BinaryValue = 0;
    int8_T BytePos;          //开始操作的字节
    int8_T BitOffset;        //在byte中的起始bit
    int8_T ValuePos;         //当前处理的bit位置
    int8_T DealLen;          //处理的数据位数
    int8_T RemainLen;        //剩余未处理的数据位数
    uint8_T byValue;

    if (data == NULL)
    {
        return;
    }
    tmp = (value - offset) / factor;
    switch(ValueMsg & 0xFF)
    {
        case 0: //interge
        {
            if (!((ValueMsg>>8) & 0xFF))    //无符号型
            {
                if (tmp < 0)
                {
                    tmp = 0;
                }
            }
            BinaryValue = (int32_T)tmp;
        }
        break;
        case 1: //float
        {
            BinaryValue = (int32_T)tmp;
        }
        break;
    }
    BytePos = startBit >> 3;
    RemainLen = BitLength;
    BitOffset = startBit - (BytePos<<3);
    ValuePos = 0;
    if ((ValueMsg>>16) & 0xFF)
    {
        do
        {
            DealLen = (RemainLen < 8-BitOffset) ? RemainLen : (8-BitOffset);
            byValue = (uint8_T)(BinaryValue >> ValuePos);
            SetBitOfU8Value(data + BytePos, DealLen, BitOffset, byValue);
            BytePos++;
            ValuePos += DealLen;
            RemainLen -= DealLen;
            BitOffset = 0;
        }while (RemainLen);
    }
    else
    {
        do
        {
            DealLen = (RemainLen < 8-BitOffset) ? RemainLen : (8-BitOffset);
            byValue = (uint8_T)(BinaryValue >> ValuePos);
            SetBitOfU8Value(data + BytePos, DealLen, BitOffset, byValue);
            BytePos--;
            ValuePos += DealLen;
            RemainLen -= DealLen;
            BitOffset = 0;
        }while (RemainLen);
    }
}

void setCanMsg3(APEX_BYTE* msg)
{
    APEX_BYTE* msg_buf = msg;
    if (NULL == msg)
    {
        printf ("Set_can_msg3 error : %s\n",codeToStr(INVALID_PARAM));
    }
    uint32_T datamsg = GetCanSigMsg(0U, 0U, 0U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_BMS_WakeupMode, msg_buf, 3, 2, datamsg, 1.0, 0.0);
    datamsg = GetCanSigMsg(0U, 0U, 1U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_HeatCoolMaxPwrCmsp, msg_buf, 8, 9, datamsg, 0.04, 0.0);
    CanPack((real32_T)AswTest_Y.Rte_VCU_MotoroutputCoolantTemp, msg_buf, 32, 8, datamsg, 1.0, -40.0);
    datamsg = GetCanSigMsg(0U, 0U, 0U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_PwCltPmpActSpdDt, msg_buf, 41, 7, datamsg, 1.0, 0.0);
    CanPack((real32_T)AswTest_Y.Rte_VCU_3_AliveCnt, msg_buf, 48, 4, datamsg, 1.0, 0.0);
    CanPack((real32_T)AswTest_Y.Rte_VCU_ChrgEna_AC, msg_buf, 52, 2, datamsg, 1.0, 0.0);
    CanPack((real32_T)AswTest_Y.Rte_VCU_3_Chksum, msg_buf, 56, 8, datamsg, 1.0, 0.0);
    printf("Set_can_msg3 : 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
        msg[0], msg[1], msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]);
}

void setCanMsg4(APEX_BYTE* msg)
{
    if (NULL == msg)
    {
        printf ("Set_can_msg4 error : %s\n",codeToStr(INVALID_PARAM));
    }
    APEX_BYTE* msg_buf = msg;
    uint32_T datamsg = GetCanSigMsg(0U, 0U, 0U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_ChrgAllowCmd, msg_buf, 3, 2, datamsg, 1.0, 0.0);
    CanPack((real32_T)AswTest_Y.Rte_VCU_OBCOprtCmd, msg_buf, 5, 3, datamsg, 1.0, 0.0);
    datamsg = GetCanSigMsg(0U, 0U, 1U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_MaxChrgVol_AC, msg_buf, 8, 11, datamsg, 0.5, 0.0);
    datamsg = GetCanSigMsg(0U, 0U, 0U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_BMS_Shutdown, msg_buf, 26, 2, datamsg, 1.0, 0.0);
    CanPack((real32_T)AswTest_Y.Rte_VCU_ChrgEna_DC, msg_buf, 28, 1, datamsg, 1.0, 0.0);
    datamsg = GetCanSigMsg(0U, 0U, 1U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_MaxChrgVol_DC, msg_buf, 29, 11, datamsg, 0.5, 0.0);
    CanPack((real32_T)AswTest_Y.Rte_VCU_MaxChrgCurnt_AC, msg_buf, 32, 10, datamsg, 0.1, 0.0);
    datamsg = GetCanSigMsg(0U, 0U, 0U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_4_AliveCnt, msg_buf, 48, 4, datamsg, 1.0, 0.0);
    datamsg = GetCanSigMsg(0U, 0U, 1U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_MaxChrgCrnt_DC, msg_buf, 52, 12, datamsg, 0.1, 0.0);
    datamsg = GetCanSigMsg(0U, 0U, 0U);
    CanPack((real32_T)AswTest_Y.Rte_VCU_4_Chksum, msg_buf, 56, 8, datamsg, 1.0, 0.0);
    printf("Set_can_msg4 : 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
        msg[0], msg[1], msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]);
}
//end

/****************************************************************\
End
\****************************************************************/
void printLog(MESSAGE_SIZE_TYPE LENGTH,MESSAGE_ADDR_TYPE MESSAGE_ADDR){
	UINT32 index = 0;
	UINT32 count =0;
	unsigned char*p = MESSAGE_ADDR;
	printf("*** Gecko:Print Can Frame!\n");
	if(LENGTH){
		for(index = 0;index < LENGTH;index++){
			printf("%x	",*p++);
			//printf("%c	",'A');
			count++;
			if(!(count%5)) {
				count = 0;
				printf("\n");
			}
		}
		printf("\n\n");
	}
}


    /* getMotorStatus: Returns the status of the motor (LC_MOTOR_STATUS_ON,
 *                 LC_MOTOR_STATUS_OFF, or LC_NO_MOTOR_STATUS
 */
UINT32 getMotorStatus ( void )
{
    MESSAGE_SIZE_TYPE lenMsgData = 0;
    VALIDITY_TYPE validity;
    RETURN_CODE_TYPE retCode;
    LC_DISCRETE_OUTPUT_RESPONSE_TYPE rdcMotorDiscreteStatus;
    UINT32 returnValue = 0;

    /*Read the MotorStatus_In Sampling Port*/
    READ_SAMPLING_MESSAGE ( sMotorStatus_In_ID,
                            ( MESSAGE_ADDR_TYPE ) & rdcMotorDiscreteStatus,
                            &lenMsgData,
                            &validity,
                            &retCode );

/* If built for debugging, always show message details */
#ifdef LC_DEBUG_MODE
    printf("\nGls: Got %d bytes, retcode %s, validity: %s\n\n", (int)lenMsgData, codeToStr(retCode), validity==VALID?"VALID":"INVALID");
    printLog(lenMsgData,(MESSAGE_ADDR_TYPE)&rdcMotorDiscreteStatus);
#endif

    if ( (retCode != NO_ACTION) || (validity != INVALID) )  /* if(port is not empty) */
    {
        checkCode ( "getMotorStatus", retCode ); /* Check for error */
    }

    if ( (retCode == NO_ERROR) && (lenMsgData > 0) && (rdcMotorDiscreteStatus.FSS.FS1 == FS_NORMAL_OPERATION) )
    {
        returnValue = ntohl( rdcMotorDiscreteStatus.RESPONSE.STATE ) & MOTOR_STATUS_BIT;
    }
    else
    {
/* If not built for debugging, show message details on error */
#ifndef LC_DEBUG_MODE
        printf("\ngetMotorStatus Error:\n");
        printf(" rx'd %d bytes, retCode is %s, validity is %s\n\n", (int)lenMsgData, codeToStr(retCode), validity==VALID?"VALID":"INVALID");
#endif
        returnValue = LC_NO_MOTOR_STATUS;
    }
    return returnValue;

}

//lihy
static CAN_DISCRETE_OUTPUT_COMMAND_TYPE CanMsg3 =
{
    /* RESERVED */
    {
        MESSAGE_RESERVED_CONSTANT /* RESERVED */
    },
    /* FSS */
    {
        FS_NORMAL_OPERATION, /* FS1 */
        FS_NO_DATA,          /* FS2 */
        FS_NO_DATA,          /* FS3 */
        FS_NO_DATA           /* FS4 */
    },
    /* CAN */
    {
        0x7700,
        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
        0x0800
    }
};

static CAN_DISCRETE_OUTPUT_COMMAND_TYPE CanMsg4 =
{
    /* RESERVED */
    {
        MESSAGE_RESERVED_CONSTANT /* RESERVED */
    },
    /* FSS */
    {
        FS_NORMAL_OPERATION, /* FS1 */
        FS_NO_DATA,          /* FS2 */
        FS_NO_DATA,          /* FS3 */
        FS_NO_DATA           /* FS4 */
    },
    /* CAN */
    {
        0x8100,
        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
        0x0800
    }
};

static CAN_DISCRETE_OUTPUT_COMMAND_TYPE CanMsg5 =
{
    /* RESERVED */
    {
        MESSAGE_RESERVED_CONSTANT /* RESERVED */
    },
    /* FSS */
    {
        FS_NORMAL_OPERATION, /* FS1 */
        FS_NO_DATA,          /* FS2 */
        FS_NO_DATA,          /* FS3 */
        FS_NO_DATA           /* FS4 */
    },
    /* CAN */
    {
        0x0500,
        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
        0x0800
    }
};

void SendCanFrame(void)
{
    MESSAGE_SIZE_TYPE lenMsgData = sizeof(CAN_DISCRETE_OUTPUT_COMMAND_TYPE);
    RETURN_CODE_TYPE retCode;
    //set can msg
    setCanMsg3(CanMsg3.CAN_MSG.MSG);
    setCanMsg4(CanMsg4.CAN_MSG.MSG);
    //send
    SEND_QUEUING_MESSAGE ( qMotorCommand_Out_ID,
                           ( MESSAGE_ADDR_TYPE ) & CanMsg3,
                           lenMsgData,
                           INFINITE_TIME_VALUE,
                           &retCode );
    checkCode("sendCan3Frame", retCode);
    usleep(100);
    SEND_QUEUING_MESSAGE ( qMotorCommand_Out_ID,
                           ( MESSAGE_ADDR_TYPE ) & CanMsg4,
                           lenMsgData,
                           INFINITE_TIME_VALUE,
                           &retCode );
    checkCode("sendCan4Frame", retCode);
};
//end

/* codeToStr: Returns error strings for A653 error codes */
UINT32 ReciveCanFrame ( void )
{
    MESSAGE_SIZE_TYPE lenMsgData = 0;
    VALIDITY_TYPE validity;
    RETURN_CODE_TYPE retCode;
    //LC_DISCRETE_INPUT_TYPE rdcSwitchDiscreteStatus;
    CAN_DISCRETE_OUTPUT_RESPONSE_TYPE rduCanDiscreteStatus;
    UINT32 returnValue = LC_SWITCH_STATUS_OFF;
    CAN_Packet packet;
    unsigned char can_msg[13];
    unsigned char *p = NULL;

    /*Read the SwitchStatus_In*/
    READ_SAMPLING_MESSAGE ( sSwitchStatus_In_ID,
                            ( MESSAGE_ADDR_TYPE ) & rduCanDiscreteStatus,//( MESSAGE_ADDR_TYPE ) & rdcSwitchDiscreteStatus,
                            &lenMsgData,
                            &validity,
                            &retCode );

#ifdef LC_DEBUG_MODE
    printf("\nGss: Got %d bytes, retcode %s, validity: %s\n\n", (int)lenMsgData, codeToStr(retCode), validity==VALID?"VALID":"INVALID");
    printLog(lenMsgData,(MESSAGE_ADDR_TYPE)&rduCanDiscreteStatus);
#endif

    if ( (retCode != NO_ACTION) || (validity != INVALID) )  /* if (port is not empty) */
    {
        checkCode ( "ReciveCanFrame", retCode ); /* Check for error */
    }

    if ( (retCode == NO_ERROR) && (lenMsgData > 0) && (rduCanDiscreteStatus.FSS.FS1 == FS_NORMAL_OPERATION) )
    {
        memset(can_msg,0,sizeof(can_msg));
        p = (unsigned char*)(&rduCanDiscreteStatus);
        memcpy(can_msg,p+40,sizeof(can_msg)-1);
        printf("can frame copy %p %p\n",p,p+40);
        printLog(12,can_msg);
        packet.id = ((unsigned int)can_msg[0]<<8) | can_msg[1];
        memcpy(packet.data,&can_msg[2],8);
        if(packet.id==0x12A){
            parse_can_message1(packet.data,8,&AswTest_B);
        }else if(packet.id==0x1BA){
            parse_can_message2(packet.data,8,&AswTest_B);
        }else if (packet.id == 0x05){
            //send msg5
            CanMsg5.CAN_MSG.CANID = packet.id;
            memcpy(CanMsg5.CAN_MSG.MSG, packet.data, 8);
            SEND_QUEUING_MESSAGE(qMotorCommand_Out_ID,
                           (MESSAGE_ADDR_TYPE) & CanMsg5,
                           lenMsgData,
                           INFINITE_TIME_VALUE,
                           &retCode);
        }else{
            printf("UNKNOW PACK\n");
        }
    }
    else
    {
/* If not built for debugging, show message details on error */
#ifndef LC_DEBUG_MODE
        printf("\nReciveCanFrame Error:\n");
        printf(" rx'd %d bytes, retCode is %s, validity is %s\n\n", (int)lenMsgData, codeToStr(retCode), validity==VALID?"VALID":"INVALID");
#endif
        returnValue = LC_NO_SWITCH_STATUS;
    }

    return returnValue;
}

void commandMotor( const UINT32 motorCommand )
{
#if 0
    MESSAGE_SIZE_TYPE lenMsgData = sizeof(LC_DISCRETE_OUTPUT_COMMAND_TYPE);
    RETURN_CODE_TYPE retCode;

    static LC_DISCRETE_OUTPUT_COMMAND_TYPE rdcMotorDiscreteCommand =
    {
        /* RESERVED */
        {
            MESSAGE_RESERVED_CONSTANT /* RESERVED */
        },
        /* FSS */
        {
            FS_NORMAL_OPERATION, /* FS1 */
            FS_NO_DATA,          /* FS2 */
            FS_NO_DATA,          /* FS3 */
            FS_NO_DATA           /* FS4 */
        },
        /* COMMAND */
        {
            0x00000000  /* BITS */
        }
    };

    rdcMotorDiscreteCommand.COMMAND.BITS = htonl( motorCommand );
#else
    static UINT16 canid = 0x0001;
    MESSAGE_SIZE_TYPE lenMsgData = sizeof(CAN_DISCRETE_OUTPUT_COMMAND_TYPE);
    RETURN_CODE_TYPE retCode;

    static CAN_DISCRETE_OUTPUT_COMMAND_TYPE rdcMotorDiscreteCommand =
    {
        /* RESERVED */
        {
            MESSAGE_RESERVED_CONSTANT /* RESERVED */
        },
        /* FSS */
        {
            FS_NORMAL_OPERATION, /* FS1 */
            FS_NO_DATA,          /* FS2 */
            FS_NO_DATA,          /* FS3 */
            FS_NO_DATA           /* FS4 */
        },
        /* CAN */
        {
            0x0001,
            {0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01},
            0x0023
        }
    };
    rdcMotorDiscreteCommand.CAN_MSG.CANID = canid++;
#endif
    SEND_QUEUING_MESSAGE ( qMotorCommand_Out_ID,
                           ( MESSAGE_ADDR_TYPE ) & rdcMotorDiscreteCommand,
                           lenMsgData,
                           INFINITE_TIME_VALUE,
                           &retCode );

#ifdef LC_DEBUG_MODE
    printf ( "\ncommandMotor: Command: %s, retCode: %s \n\n", 
             ( (motorCommand & MOTOR_COMMAND_BIT) > 0 ) ? "LC_MOTOR_COMMAND_ON" : "LC_MOTOR_COMMAND_OFF", 
             codeToStr(retCode) );
#else
    checkCode ( "commandMotor", retCode );
#endif
}
void timerControllerProcessA( void )
{
    RETURN_CODE_TYPE retCode;
    static unsigned int counter = 0;
    printf( "timer Controller process starting..\n" );
    FOREVER
    {
	    printf("Hello World %d\n",counter);
        sample_usage_Input_10ms();
        sample_usage_Strgy_10ms();
        sample_usage_Output_10ms();
        printf("sample_usage_Strgy_10ms\n");
        printf("Rte_VCU_4_AliveCnt %d",AswTest_Y.Rte_VCU_4_AliveCnt);
	    counter++;
	    if(counter%2==0){
            sample_usage_Strgy_20ms();
	        printf("sample_usage_Strgy_20ms\n");
            printf("Rte_VCU_MaxChrgCurnt_AC %f\n",AswTest_Y.Rte_VCU_MaxChrgCurnt_AC);
        }
        if(counter%5==0){
            printf("sample_usage_Strgy_50ms\n");
            printf("Rte_VCU_MaxChrgCrnt_DC %f\n",AswTest_Y.Rte_VCU_MaxChrgCrnt_DC);
        }
        if(counter%10==0){
            printf("sample_usage_Strgy_100ms\n");
            printf("Rte_VCU_3_AliveCnt %d\n",AswTest_Y.Rte_VCU_3_AliveCnt);
            counter = 0;
        }
	    PERIODIC_WAIT ( &retCode );
	    checkCode ( "PERIODIC_WAIT", retCode );
    }
}

#define CYCLE_MAX 500
/* Switch Register Periodic Process */
void motorControllerProcess( void )
{
    //UINT32 switchStatus;
    //UINT32 motorStatus;
    RETURN_CODE_TYPE retCode;
    UINT32 cycle_count=0;

    printf( "motor Controller process starting..\n" );
    FOREVER
    {
        if(!(cycle_count%CYCLE_MAX)){
            cycle_count = 0;	
            ReciveCanFrame();
            SendCanFrame();
            /************************************/
            /* Wait till next period expiration */
            /************************************/
            PERIODIC_WAIT ( &retCode );
            checkCode ( "PERIODIC_WAIT", retCode );
        }
        cycle_count += 1;
        usleep(1000);
    }
}

void motorControllerStart( void )
{
    LOCAL PROCESS_ID_TYPE controllerProcessIdA;
    RETURN_CODE_TYPE retCodeA;
    const PROCESS_ATTRIBUTE_TYPE timerProcessAttributeA =
    {
        "time_test",                 /* Name          */
        timerControllerProcessA,      /* Entry point   */
        8000,                        /* Stack size    */
        100,                         /* Priority      */
        1000000000,                  /* Period   1s     */
        30000000,                    /* Time capacity  30ms*/
        SOFT                         /* Deadline type */
    };


    LOCAL PROCESS_ID_TYPE controllerProcessId;
    RETURN_CODE_TYPE retCode;

    const PROCESS_ATTRIBUTE_TYPE controllerProcessAttribute =
    {
        "tLCProc",                  /* Name          */
        motorControllerProcess,     /* Entry point   */
        LC_PROC_STACK_SIZE,         /* Stack size    */
        LC_PROC_PRIORITY,           /* Priority      */
        0,//LC_PROC_PERIOD,             /* Period        */
        0,//LC_PROC_TIME_CAP,           /* Time capacity */
        SOFT                        /* Deadline type */
    };

    (STATUS) scoeAmioEnable(); /* Enable AMIO */

    printf( "\nCreating ports ...\n" );

    CREATE_SAMPLING_PORT ( "MotorStatus_In_Dest",
                            LC_MOTOR_STATUS_MSG_SIZE,
                            DESTINATION,
                            LC_SAMPLING_REFRESH_RATE,
                            &sMotorStatus_In_ID,
                            &retCode );

    checkCode("CREATE_SAMPLING_PORT (MotorStatus_In_Dest)", retCode);

    CREATE_SAMPLING_PORT ( "SwitchStatus_In_Dest",
                           LC_SWITCH_STATUS_MSG_SIZE,
                           DESTINATION,
                           LC_SAMPLING_REFRESH_RATE,
                           &sSwitchStatus_In_ID,
                           &retCode );

    checkCode("*** Gecko:CREATE_SAMPLING_PORT (SwitchStatus_In_Dest)", retCode);

    CREATE_QUEUING_PORT ( "MotorCommand_Out_Src",
                          LC_MOTOR_COMMAND_MSG_SIZE,
                          LC_MOTOR_COMMAND_QUEUE_DEPTH,
                          SOURCE,
                          FIFO,
                          &qMotorCommand_Out_ID,
                          &retCode );

    checkCode("CREATE_QUEUING_PORT (MotorCommand_Out_Src)", retCode);

    /**************************/
    /* Create control process */
    /**************************/
    printf( "Creating control process ...\n" );

    CREATE_PROCESS ( (PROCESS_ATTRIBUTE_TYPE*) &controllerProcessAttribute,   /* process attribute */
                     &controllerProcessId,       /* process Id        */
                     &retCode );

    checkCode("CREATE_PROCESS (tLCProc) ", retCode);


    printf( "Creating timerA process ...\n" );
    CREATE_PROCESS ( (PROCESS_ATTRIBUTE_TYPE*) &timerProcessAttributeA,   /* process attribute */
                     &controllerProcessIdA,       /* process Id        */
                     &retCodeA );
    checkCode("CREATE_PROCESS (timerAprocess)", retCode);
	/***********************
	Initialize model
	*/
	AswTest_initialize();
	printf("AswTest_initialize end\n");

    /*************************/
    /* start control process */
    /*************************/
    printf( "Starting processes ...\n" );

    START ( controllerProcessId, &retCode );
    START ( controllerProcessIdA, &retCodeA );

    checkCode("START (tLCProc)", retCode);
    checkCode("START (timerprocess)", retCode);

    /**************************************************/
    /* enter NORMAL partition mode to begin operation */
    /**************************************************/
    printf( "Entering normal mode ... " );

    SET_PARTITION_MODE ( NORMAL, &retCode );

    checkCode ( "SET_PARTITION_MODE", retCode );

	/**************
	*/
	while (rtmGetErrorStatus(AswTest_M) == (NULL)) {
	  /*  Perform application tasks here. */
	}
	
	/* Terminate model */
	AswTest_terminate();

    return;
}


